#include "controller/mc_config.h"
#include "config.h"
#include "mot_params.h"
#include "bsp/bsp.h"

mc_config_t _mc_conf;

static void mc_conf_calc_dqloop(void) {
	_mc_conf.c.pid[pid_id].kp = hz_2_w_rads(_mc_conf.c.dq_loop_bandwith) * _mc_conf.m.ld;
	_mc_conf.c.pid[pid_id].ki = hz_2_w_rads(_mc_conf.c.dq_loop_bandwith) * _mc_conf.m.rs;

	_mc_conf.c.pid[pid_iq].kp = hz_2_w_rads(_mc_conf.c.dq_loop_bandwith) * _mc_conf.m.lq;
	_mc_conf.c.pid[pid_iq].ki = hz_2_w_rads(_mc_conf.c.dq_loop_bandwith) * _mc_conf.m.rs;
}

void mc_conf_init(void) {
	_mc_conf.pwm_freq = CONFIG_FOC_PWM_FREQ;
	_mc_conf.pwm_period = CONFIG_FOC_PWM_period(CONFIG_FOC_PWM_FREQ);
	_mc_conf.pwm_half_period = _mc_conf.pwm_period >> 1;
	_mc_conf.ts = 1.0f/_mc_conf.pwm_freq;
#if CONFIG_FOC_PWM_UPDATE_REPEAT==0
	_mc_conf.ts = _mc_conf.ts / 2.0f;
#endif
	_mc_conf.slow_ts = 1.0f/(float)CONFIG_SPEED_CTRL_FREQ;

	_mc_conf.m.poles = CONFIG_MOTOR_PARAMS_Poles;
	_mc_conf.m.ld = CONFIG_MOTOR_PARAMS_Ld;
	_mc_conf.m.lq = CONFIG_MOTOR_PARAMS_Lq;
	_mc_conf.m.rs = CONFIG_MOTOR_PARAMS_Rs;
	_mc_conf.m.flux = CONFIG_MOTOR_PARAMS_Flux;
	_mc_conf.m.J = CONFIG_MOTOR_PARAMS_J;
	_mc_conf.m.max_rpm = CONFIG_MOTOR_PARAMS_MAX_RPM;
	
	_mc_conf.m.encoder_offset = CONFIG_MOTOR_ENCODER_OFFSET;

	_mc_conf.b.vbus_voltage_max = CONFIG_HW_MAX_DC_VOL;
	_mc_conf.b.vbus_voltage_min = CONFIG_HW_MIN_DC_VOL;
	_mc_conf.b.vbus_curr_max = CONFIG_HW_MAX_DC_CURRENT;
	_mc_conf.b.phase_curr_max = CONFIG_HW_MAX_PHASE_CURR;

	_mc_conf.c.dq_loop_bandwith = 300;
	_mc_conf.c.enc_pll_bandwith = 300;

	mc_conf_calc_dqloop();

	_mc_conf.c.pid[pid_vel].kp = 0.236f;
	_mc_conf.c.pid[pid_vel].ki = 0.06f;
}
